'''Date : O5th october 2012
	 Authors : Isaline Laurent (ens12ilt) - Gizem Berkdemir (ens12gbr)

	 This file contains the function used to get the carrot point 
	 '''


import json, time, httplib
from tools import *
from math import pi, atan2

HEADERS = {"Content-type": "application/json", "Accept": "text/json"}
URL = 'localhost:50000'

def getNextPoint(rpath, lookahead, limit) :
		'''Get the closest point from a looking ahead distance, into the json path
				 Input : rpath, the loaded json path converted into the robot coordinate system
								 lookahead, a given look ahead distance
								 limit, the number of point used in the original path
				 Output : d, the distance between the chosen point and the robot
									j, the index of this point in the path
									a, the angle between the chosen point and the robot
		'''
		d = 10*lookahead 
		j = -1 
		a = 0
		p = 0

		for i in range(len(rpath)) :
				di = getDistance(rpath[i])
				ai = atan2(rpath[i][1], rpath[i][0])
				if (di >= lookahead and di*(abs(ai) + 2) < d*(abs(a) + 2)) :
						j = i
						d = di
						a = ai
		
		if j == -1 :
				for i in range(len(rpath)) :
						di = getDistance(rpath[i])
						ai = atan2(rpath[i][1], rpath[i][0])
						if (di >= lookahead and di < d) :
								j = i
								d = di
								a = ai

		if j == -1 :
				d = getDistance(rpath[0])
				p = 1
				for i in range(len(rpath)) :
						di = getDistance(rpath[i])
						ai = atan2(rpath[i][1], rpath[i][0])
						if (di >= d) :
								j = i
								d = di
								a = ai

		return d, j, a, p



def deleteObsPoints(path, i) :
		''' Delete the exceeded points from the path, note that the last carrot point is not deleted
					Input : path, the python object path
									i, the last index to delete (not included)
		'''
		for z in range(i) :
				del path[0]
		if i == 0:	
				del path[i]

def loadPath(file) :
		''' Load a path given by a json file.
					Input : file, the path to the json file
					Output : a python object containing the loaded file
		'''
		json_file = open(file).read()
		return json.loads(json_file)
